#include <caros/kuka_lwr_msgs.h>
#include<string>
namespace caros
{
std::string to_string(rw::math::Q q)
{
  std::string out = std::to_string(q[0]);
  for (size_t i = 1; i < q.size(); ++i)
  {
    out += " " + std::to_string(q[i]);
  }
  return out;
}

std::string to_string(rw::math::Transform3D<double> T)
{
  std::string out = std::to_string(T.P()(0) * 1000);  // pose is in mm
  for (int i = 1; i < 3; ++i)
  {
    out += " " + std::to_string(T.P()(i) * 1000);  // pose is in mm
  }
  rw::math::RPY<double> rpy(T.R());
  for (int i = 0; i < 3; ++i)
  {
    out += " " + std::to_string(rpy(i));
  }
  return out;
}

std::string to_string(geometry_msgs::WrenchStamped w)
{
  std::string out = std::string("1000");  // std::to_string(w.wrench.force.x);
  out += " " + std::string("1000");       // std::to_string(w.wrench.force.y);
  out += " " + std::string("1000");       // std::to_string(w.wrench.force.z);
  out += " " + std::string("1000");       // std::to_string(w.wrench.torque.x);
  out += " " + std::string("1000");       // std::to_string(w.wrench.torque.y);
  out += " " + std::string("1000");       // std::to_string(w.wrench.torque.z);
  return out;
}

// std::string smart_move_q(rw::math::Q target, rw::math::Q velocity = rw::math::Q(7,1.0,1.0,1.0,1.0,1.0,1.0,1.0))
// {
//     std::string msg = "smart joint move : " + to_string(target) + " " + to_string(velocity);
//     return msg;
// }
// std::string direct_move_q(rw::math::Q target, rw::math::Q velocity = rw::math::Q(7,1.0,1.0,1.0,1.0,1.0,1.0,1.0))
// {
//     std::string msg = "direct joint move : " + to_string(target) + " " + to_string(velocity);
//     return msg;
// }
std::string move_q(rw::math::Q target, rw::math::Q velocity, double blend)
{
  std::string msg = "joint move : " + to_string(target) + " " + to_string(velocity) + " " + std::to_string(blend);
  return msg;
}
std::string move_q(rw::math::Q target, rw::math::Q velocity)
{
  std::string msg = "joint move : " + to_string(target) + " " + to_string(velocity);
  return msg;
}
std::string move_q(rw::math::Q target)
{
  std::string msg = "joint move : " + to_string(target);
  return msg;
}
// std::string smart_move(rw::math::Transform3D<double> target, double velocity)
// {
//     std::string msg = "smart cartesian move : " + to_string(target) + " " + std::to_string(velocity);
//     return msg;
// }
// std::string direct_move(rw::math::Transform3D<double> target, double velocity)
// {
//     std::string msg = "direct cartesian move : " + to_string(target) + " " + std::to_string(velocity);
//     return msg;
// }
std::string lin_move(rw::math::Transform3D<double> target, double velocity, double blend)
{
  std::string msg = "lin move : " + to_string(target) + " " + std::to_string(velocity) + " " + std::to_string(blend);
  return msg;
}
std::string ptp_move(rw::math::Transform3D<double> target, double velocity, double blend)
{
  std::string msg = "ptp move : " + to_string(target) + " " + std::to_string(velocity) + " " + std::to_string(blend);
  return msg;
}

std::string ptp_move_path(rw::math::Q target, double velocity, double blend, bool first, bool last)
{
  std::string msg = "ptp move path : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + (first ? " 1 " : " 0 ") + (last ? " 1 " : " 0 ");
  return msg;
}

std::string linr_move(rw::math::Transform3D<double> target, double velocity, double blend, double redundancy)
{
  std::string msg = "linr move : " + to_string(target) + " " + std::to_string(velocity) + " " + std::to_string(blend) +
                    " " + std::to_string(redundancy);
  return msg;
}
std::string ptpr_move(rw::math::Transform3D<double> target, double velocity, double blend, double redundancy)
{
  std::string msg = "ptpr move : " + to_string(target) + " " + std::to_string(velocity) + " " + std::to_string(blend) +
                    " " + std::to_string(redundancy);
  return msg;
}
std::string linforcex_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity,
                           double blend)
{
  std::string msg = "linforcex move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string linforcey_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity,
                           double blend)
{
  std::string msg = "linforcey move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string linforcez_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity,
                           double blend)
{
  std::string msg = "linforcez move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string forcex_move(rw::math::Transform3D<double> target, double force_low, double force_high, double velocity,
                        double blend)
{
  std::string msg = "forcex move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_low) + " " + std::to_string(force_high);
  return msg;
}
std::string forcey_move(rw::math::Transform3D<double> target, double force_low, double force_high, double velocity,
                        double blend)
{
  std::string msg = "forcey move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_low) + " " + std::to_string(force_high);
  return msg;
}
std::string forcez_move(rw::math::Transform3D<double> target, double force_low, double force_high, double velocity,
                        double blend)
{
  std::string msg = "forcez move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_low) + " " + std::to_string(force_high);
  return msg;
}
std::string ptpforcez_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity,
                           double blend)
{
  std::string msg = "ptpforcez move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string linforce_move(rw::math::Transform3D<double> target, double force_threshold_x, double force_threshold_y,
                          double force_threshold_z, double velocity, double blend)
{
  std::string msg = "linforce move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_x) + " " +
                    std::to_string(force_threshold_y) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string ptpforce_move(rw::math::Transform3D<double> target, double force_threshold_x, double force_threshold_y,
                          double force_threshold_z, double velocity, double blend)
{
  std::string msg = "ptpforce move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(force_threshold_x) + " " +
                    std::to_string(force_threshold_y) + " " + std::to_string(force_threshold_z);
  return msg;
}
std::string ptpstiff_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance, double velocity)
{
  std::string msg =
      "ptpstiff move : " + to_string(target) + " " + std::to_string(velocity) + " " + to_string(impedance);
  return msg;
}
std::string linstiff_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                          double stiffness, double velocity)
{
  std::string msg = "linstiff move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(stiffness) + " " + to_string(impedance);
  return msg;
}
std::string ptpcompliant_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                              double velocity, double blend)
{
  std::string msg = "ptpcompliant move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + to_string(impedance);
  return msg;
}
std::string lincompliant_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                              double stiffness, double velocity, double blend)
{
  std::string msg = "lincompliant move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + std::to_string(stiffness) + " " + to_string(impedance);
  return msg;
}
std::string qcompliant_move(rw::math::Q target, geometry_msgs::WrenchStamped impedance, double velocity, double blend)
{
  std::string msg = "qcompliant move : " + to_string(target) + " " + std::to_string(velocity) + " " +
                    std::to_string(blend) + " " + to_string(impedance);
  return msg;
}
std::string start_gravity(double stiffness)
{
  std::string msg = "start gravity : " + std::to_string(stiffness);
  return msg;
}
std::string stop()
{
  std::string msg = "stop";
  return msg;
}
std::string update()
{
  std::string msg = "hello";
  return msg;
}
std::string exit()
{
  std::string msg = "bye";
  return msg;
}
std::string set_velocity(double speed)
{
  std::string msg = "velocity : " + std::to_string(speed);
  return msg;
}

}  // namespace caros
